import abc
import copy

import numpy as np

import spatialmath as sm


def fkine(M: sm.SE3, S_list: list, q_list: list) -> sm.SE3:
    T = M.copy()
    for i in range(len(q_list) - 1, -1, -1):
        T = sm.Twist3(np.hstack((S_list[i][3:], S_list[i][:3])) * q_list[i]) * T
    return T


class BipedRobot(abc.ABC):

    def __init__(self) -> None:
        super().__init__()
        self.dof = 0
        self.T = ()
        self.Ms = []
        self.q = []
        self.S_list1 = []
        self.S_list2 = []

    def set_joint(self, q: list):
        self.q = q[:]
        self.update_cartesian()

    def get_joint(self):
        return self.q[:]

    def move(self, parameter):
        pass

    def set_cartesian(self, left, right):
        self.set_joint(self.ikine(left, right))

    def get_cartesian(self) -> tuple:
        return copy.deepcopy(self.T)

    def update_cartesian(self) -> None:
        self.T = self.fkine(self.q)

    @abc.abstractmethod
    def ikine(self, left, right) -> list:
        pass

    @abc.abstractmethod
    def fkine(self, q_list: list) -> tuple:
        q_list1 = q_list[0:len(q_list) // 2]
        q_list2 = q_list[len(q_list) // 2:]
        return fkine(self.Ms[0], self.S_list1, q_list1), fkine(self.Ms[1], self.S_list2, q_list2)
